/** ###################################################################
**     Filename  : Test.c
**     Project   : parking
**     Processor : MC9S12XS128MAL
**     Compiler  : CodeWarrior HCS12X C Compiler
**     Date/Time : 2013-3-9, 19:25
**     Contents  :
**         User source code
**
** ###################################################################*/

/* MODULE Test */

#include "Test.h"
#include "ParkControl.h"
#include "CommandSystem.h"
#include "Hmi.h"
#include "Communication.h"
#include "Drv.h"
#include "Em.h"
#include "Lights.h"
#include <stdio.h>

void TstSetParkControl(
	S16 seq, S16 mode, S16 dir, S16 distance, S16 speed, S16 trig_min, S16 extended){
	if (seq < CS_MAX_COMMAND){		
		PCProcedure[seq].mode = mode;
		PCProcedure[seq].direction = dir;
		PCProcedure[seq].distance = distance;
		PCProcedure[seq].speed = speed;
		PCProcedure[seq].trig_min = trig_min;
		PCProcedure[seq].extended = extended;
	}
}

void TstInitParkControl(void){
	U8 i = 0;
//				seq		mode				dir			dis	  speed min ex
/*	TstSetParkControl(i++,	CS_AUTO_FORWARD,	0,		160,	20,	99,	20);
	TstSetParkControl(i++,	CS_AUTO_FORWARD,	0,		15,		20,	0,	0);
	TstSetParkControl(i++,	CS_STOP,			0,		2,		0,	0,	0);
	TstSetParkControl(i++,	CS_BACKWARD,		487,	61,		20,	0,	0);
	TstSetParkControl(i++,	CS_STOP,			0,		2,		0,	0,	0);
	TstSetParkControl(i++,	CS_FORWARD,			-363,	33,		20,	0,	0);
	TstSetParkControl(i++,	CS_STOP,			0,		2,		0,	0,	0);
	TstSetParkControl(i++,	CS_AUTO_BACKWARD,	0,		80,		15,	0,	0);
	TstSetParkControl(i++,	CS_STOP,			0,		2,		0,	0,	0);
	TstSetParkControl(i++,	CS_MILESTONE,		0,		8,		0,	0,	0);
	TstSetParkControl(i++,	CS_FORWARD,			0,		12,		15,	0,	0);
	TstSetParkControl(i++,	CS_FORWARD,			487,	75,		20,	0,	0);
	TstSetParkControl(i++,	CS_STOP,			387,	1,		0,	0,	0);
	TstSetParkControl(i++,	CS_AUTO_FORWARD,	0,		45,		20,	0,	0);
	TstSetParkControl(i++,	CS_STOP,			0,		1,		0,	0,	0);
	TstSetParkControl(i++,	CS_AUTO_BACKWARD,	0,		180,	20,	115,0);
	TstSetParkControl(i++,	CS_AUTO_BACKWARD,	0,		45,		15,	0,	0);
	TstSetParkControl(i++,	CS_STOP,			0,		1,		0,	0,	0);
	TstSetParkControl(i++,	CS_STOP,			0,		10,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);	
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);		  */
	TstSetParkControl(i++,	CS_AUTO_FORWARD,	0,		160,	20,	73,	20);
	TstSetParkControl(i++,	CS_AUTO_FORWARD,	0,		46,		20,	0,	0);
	TstSetParkControl(i++,	CS_STOP,			460,	2,		0,	0,	0);
	TstSetParkControl(i++,	CS_BACKWARD,		460,	81,		20,	0,	0);
	TstSetParkControl(i++,	CS_STOP,			-500,	2,		0,	0,	0);
	TstSetParkControl(i++,	CS_FORWARD,			-500,	18,		20,	0,	0);
	TstSetParkControl(i++,	CS_AUTO_FORWARD,	0,		34,		20,	0,	0);
	TstSetParkControl(i++,	CS_STOP,			0,		2,		0,	0,	0);
	TstSetParkControl(i++,	CS_BACKWARD,		0,		0,		20,	0,	0);
	TstSetParkControl(i++,	CS_AUTO_BACKWARD,	0,		75,		15,	0,	0);
	TstSetParkControl(i++,	CS_STOP,			0,		2,		0,	0,	0);
	TstSetParkControl(i++,	CS_MILESTONE,		0,		8,		0,	0,	0);
	TstSetParkControl(i++,	CS_FORWARD,			0,		10,		15,	0,	0);
	TstSetParkControl(i++,	CS_FORWARD,			487,	95,		20,	0,	0);
	TstSetParkControl(i++,	CS_AUTO_FORWARD,	0,		50,		20,	0,	0);
	TstSetParkControl(i++,	CS_STOP,			0,		1,		0,	0,	0);
	TstSetParkControl(i++,	CS_AUTO_BACKWARD,	0,		165,	20,	110,0);
	TstSetParkControl(i++,	CS_AUTO_BACKWARD,	0,		50,		20,	0,	0);
	TstSetParkControl(i++,	CS_STOP,			0,		1,		0,	0,	0);
	TstSetParkControl(i++,	CS_STOP,			0,		10,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);	
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
}

void TstInitEmBackwardControl(void){
	U8 i = 0;
	TstSetParkControl(i++,	CS_AUTO_BACKWARD,	0,		160,	20,	83,	20);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	TstSetParkControl(i++,	CS_EMPTY,			0,		0,		0,	0,	0);
	

}

void TstInitStop(void){
	U8 i = 0;
	TstSetParkControl(i++, CS_AUTO_FORWARD, 0, 20, 25, 80, 0);
	TstSetParkControl(i++, CS_STOP, 0, 5, 0, 0, 0);
	TstSetParkControl(i++, CS_BACKWARD, 0, 20, 25, 0, 0);
	TstSetParkControl(i++, CS_STOP, 0, 1, 0, 0, 0);
	TstSetParkControl(i++, CS_EMPTY, 0, 1, 0, 0, 0);	
}

void TstInitParkOutControl(void){
	U8 i = 0;
	TstSetParkControl(i++, CS_AUTO_FORWARD, 0, 200, 25, 80, 0);
	TstSetParkControl(i++, CS_AUTO_FORWARD, 0, 22, 25, 0, 0);
	TstSetParkControl(i++, CS_STOP, 0, 1, 0, 0, 0);
	TstSetParkControl(i++, CS_BACKWARD, 230, 50, 25, 0, 0);
	TstSetParkControl(i++, CS_STOP, 0, 1, 0, 0, 0);
	TstSetParkControl(i++, CS_FORWARD, -170, 25, 25, 0, 0);
	TstSetParkControl(i++, CS_STOP, 0, 1, 0, 0, 0);
	TstSetParkControl(i++, CS_BACKWARD, 0, 75, 25, 0, 0);
	TstSetParkControl(i++, CS_STOP, 0, 1, 0, 0, 0);
	TstSetParkControl(i++, CS_EMPTY, 0, 1, 0, 0, 0);	
}


void TstParkControl(void){
	PCPrepare();
	PCDrive();	
}


void TstEncoderScale(void){
	U16 oldValue;
	U16 nowValue;
	S16 delta;
	S32 value = 0;
	U16 len;
	U8 c;
	char str[50];
	HMI_PUSH_HANDLER *oldHandler;
    c = HmiWaitForDigit();
    HmiNormalProcessIsRunning=1;
    oldHandler=HmiSetHandler(&HmiNormalQuitIrqHandler);
    HmiQueryPulseAccumulator(&nowValue);
    while(HmiNormalProcessIsRunning){
     	oldValue = nowValue;
    	HmiQueryPulseAccumulator(&nowValue);
    	delta = (S16)(nowValue - oldValue);
    	value += delta;   
        HmiShowNumber((U16)value);   
     	(void)sprintf(&str[0], "Distance = %ld\n", value);
     	len = strlen(str);
     	ASSendData((U8*)str, len);
     	Cpu_Delay100US(200);
    }
    HmiUnsetHandler(oldHandler);
	
					
}

void TstParkControlCalibration(void){
	U16 oldValue;
	U16 nowValue;
	S16 delta;
	S32 value = 0;
	U16 len;
	U16 c;
	U16 cc;
	U16 duty;
	char str[50];
	HMI_PUSH_HANDLER *oldHandler;
    c = HmiWaitForNumberEx(DrvServoMiddle, 10, 20, 100);
    cc = HmiWaitForNumberEx(DrvServoMiddle, 0, 99, 1);
    duty = c + cc;
    HmiNormalProcessIsRunning=1;
    oldHandler=HmiSetHandler(&HmiNormalQuitIrqHandler);
    HmiQueryPulseAccumulator(&nowValue);
    (void)DRV_SERVO_Enable();
    if (duty == 1122 || duty == 1922){
    	
    		DrvEnableMotorControl();	
    }
    while(HmiNormalProcessIsRunning){
    	if (duty == 1111){
    		EMDriveForward();
    	}else if(duty == 1122){
    		EMDriveForward();	
			DrvSetSpeedMotor(15);
    	}else if(duty == 1922){
    		EMDriveBackward();	
			DrvSetSpeedMotor(-15);
    	}else if (duty == 1911){
    		EMDriveBackward();	
    	}else{
    		DRV_SERVO_SetDuty(duty);	
    	}
     	oldValue = nowValue;
    	HmiQueryPulseAccumulator(&nowValue);
    	delta = (S16)(nowValue - oldValue);
    	value += delta;   
        HmiShowNumber((U16)value / PC_DISTANCE_SCALE);   
     	(void)sprintf(&str[0], "Distance = %ld\n", value / PC_DISTANCE_SCALE);
     	len = strlen(str);
     	ASSendData((U8*)str, len);
     	Cpu_Delay100US(200);
    }
    (void)DRV_SERVO_Disable();
    DrvDisableMotorControl();
    HmiUnsetHandler(oldHandler);			
}

void TstTestDrv(void (*fun)(void), S16 speed){
	HMI_PUSH_HANDLER *oldHandler;
	(void)HmiWaitForDigit();
	(void)DRV_SERVO_Enable();
	DrvEnableMotorControl();
	DrvSetSpeedMotor(speed);
    HmiNormalProcessIsRunning=1;
    oldHandler=HmiSetHandler(&HmiNormalQuitIrqHandler);
    while(HmiNormalProcessIsRunning){
    	(*fun)();
    }
    HmiUnsetHandler(oldHandler);			
	DrvDisableMotorControl();
	(void)DRV_SERVO_Disable();
}

void TstTestOnce(void (*fun)(void)){
	HMI_PUSH_HANDLER *oldHandler;
	(void)HmiWaitForDigit();
    HmiNormalProcessIsRunning=1;
    oldHandler=HmiSetHandler(&HmiNormalQuitIrqHandler);
    while(HmiNormalProcessIsRunning){
    	(*fun)();
    	HmiNormalProcessIsRunning = 0;	
    }
    HmiUnsetHandler(oldHandler);	
}

void TstTest(void (*fun)(void)){
	HMI_PUSH_HANDLER *oldHandler;
	(void)HmiWaitForNumberEx(11, 0, 99, 1);
    HmiNormalProcessIsRunning=1;
    oldHandler=HmiSetHandler(&HmiNormalQuitIrqHandler);
    while(HmiNormalProcessIsRunning){
    	(*fun)();	
    }
    HmiUnsetHandler(oldHandler);	
}

void TstPark(void){
	TstInitParkControl();
	TstParkControl();		
}

void TstInf(void){
	if (PCInfDetect()){
		HmiShowNumber(88);	
	}else{
		HmiShowNumber(0);	
	}
}

void TstStop(void){
	TstInitStop();
	TstParkControl();	
}

void TstEMBackward(void){
	EMDriveBackward();
}

void TstCheckFlag(void){
	char str[100];
	U16 flag;
	U16 len;
	PCGetFlagMemory(PC_FLAG_PROCEDURE_OFFSET, &flag);
	(void)sprintf(str, "flag = %u\n", flag);
	len = strlen(str);
	ASSendData((U8*)&str[0], len);
	Cpu_Delay100US(100);	
}

void TstEncoder(void){
	U16 value;
	HmiQueryPulseAccumulator(&value);
	HmiShowNumber(value / 100);			
}

void TstServoAndDrv(void){
	for (;;){
		U16 c = 0;
		U16 cc;	
		U16 duty = 0;
		(void)DRV_SERVO_Disable();
		c = HmiWaitForNumberEx(1520, 12, 18, 100);
		cc = HmiWaitForNumberEx(1520, 0, 99, 1);
		duty = c + cc;
		(void)DRV_SERVO_Enable();
		DRV_SERVO_SetDuty(duty);
		DrvEnableMotorControl();
		DrvSetSpeedMotor(0);
		(void)HmiWaitForDigit();
		DrvDisableMotorControl();
	}	 	
}

void TstDrv(void){
	for (;;){
		U16 c = 0;
		U16 cc;	
		U16 duty = 0;
		char str[100];
		U16 len;
	
		c = HmiWaitForNumberEx(32768, 0, 65, 1000);
		cc = HmiWaitForNumberEx(0, 0, 99, 10);
		duty = c + cc;
		
			
		(void)DRV_MEN_SetVal();
		(void)DRV_PWM_Enable();
		DRV_BACK_PutVal(1);

		(void)sprintf(str, "duty = %u\n", duty);
		len = strlen(str);
		(void)DRV_PWM_SetRatio16(duty);
		ASSendData((U8*)&str[0], len);
		Cpu_Delay100US(100);	
		(void)HmiWaitForDigit();
		(void)DRV_MEN_ClrVal();
		(void)DRV_PWM_Disable();
		

	}	 		
	
}

void TstEm(void){
	for (;;){
		U8 left, right,leftb, rightb;
		S16 value, valueb, slope;
		U16 len;
		char str[100];
		EMDriveForward();
		left = EMLeftFrontValue;
		right = EMRightFrontValue;
		leftb = EMLeftBackValue;
		rightb = EMRightBackValue;
		value = EMGetValueFront();
		valueb = EMGetValueBack();
		slope = valueb - value;	
		HmiShowNumber(left);
		(void)sprintf(str, "Lf = %u Rf = %u\tLb = %u Rb = %u\tVf=%d Vb = %d slope=%d\n",
			 left, right, leftb, rightb, value, valueb, slope);
		len = strlen(str);
		ASSendData((U8*)&str[0], len);
		Cpu_Delay100US(100);
	}   				
	
}

void TstEmInfluence(void){
	for (;;){
		U8 left, right,leftb, rightb;
		S16 value, valueb, slope;
		U16 len;
		char str[100];
		EMDriveForward();
		left = EMLeftFrontValue;
		right = EMRightFrontValue;
		leftb = EMLeftBackValue;
		rightb = EMRightBackValue;
		value = EMGetValueFront();
		valueb = EMGetValueBack();
		slope = valueb - value;	
		HmiShowNumber(left);
		(void)sprintf(str, "Lf = %u Rf = %u\tLb = %u Rb = %u\tVf=%d Vb = %d slope=%d\n",
			 left, right, leftb, rightb, value, valueb, slope);
		len = strlen(str);
		ASSendData((U8*)&str[0], len);
		Cpu_Delay100US(100);
	}   				
	
}


void TstDrvSpeed(void){
	HMI_PUSH_HANDLER *oldHandler;
	S16 speed;
//	U16 c, cc;
	U16 duty;
	char str[100];
	U16 len;
	for(;;){
		duty = HmiWaitForNumberEx(0, 0, 99, 10);
		speed = (S16)duty;
		DrvEnableMotorControl();
		DrvSetSpeedMotor(speed);
		(void)sprintf(str, "duty = %d\n", speed);
		len = strlen(str);
		ASSendData((U8*)&str[0], len);
		Cpu_Delay100US(100);	
	    HmiNormalProcessIsRunning=1;
	    oldHandler=HmiSetHandler(&HmiNormalQuitIrqHandler);
	    while(HmiNormalProcessIsRunning){
	    	
	    }
	    HmiUnsetHandler(oldHandler);			
		DrvDisableMotorControl();
		(void)DRV_SERVO_Disable();	
	}
	
}
void TstLights(void){
	HMI_PUSH_HANDLER *oldHandler;
	for(;;){
		U16 choice;	
		choice = HmiWaitForNumberEx(0, 0, 99, 1);
		if (choice == 1){
			LTLeftOn();	
		}else if (choice == 2){
			LTRightOn();	
		}else if (choice == 3){
			LTStopOn();		
		}else{
			LTLeftOn();
			LTRightOn();
			LTStopOn();		
		}
	    HmiNormalProcessIsRunning=1;
	    oldHandler=HmiSetHandler(&HmiNormalQuitIrqHandler);
	    while(HmiNormalProcessIsRunning){
	    	
	    }
	    LTLeftOff();
		LTRightOff();
		LTStopOff();	
	    HmiUnsetHandler(oldHandler);			
	}	
}

void TstEMDriveBackward(void){
	for(;;){
		
		HMI_PUSH_HANDLER *oldHandler;
		S16 speed;
		U16 duty;
	//	duty = HmiWaitForNumberEx(0, 0, 99, 10);
	duty = 0;
		speed = (S16)duty;
		if (speed > 0){
			DrvEnableMotorControl();
			DrvSetSpeedMotor(-speed);
		}
		(void)DRV_SERVO_Enable();	
	    HmiNormalProcessIsRunning=1;
	    oldHandler=HmiSetHandler(&HmiNormalQuitIrqHandler);
	    while(HmiNormalProcessIsRunning){
	    	U8 left, right,leftb, rightb;
			S16 value, valueb, slope;
			U16 len;
			char str[100];
			EMDriveBackward();
			left = EMLeftFrontValue;
			right = EMRightFrontValue;
			leftb = EMLeftBackValue;
			rightb = EMRightBackValue;
			value = EMGetValueFront();
			valueb = EMGetValueBack();
			slope = valueb - value;	
			HmiShowNumber(left);
			(void)sprintf(str, "Lf = %u Rf = %u\tLb = %u Rb = %u\tVf=%d Vb = %d slope=%d\n",
				 left, right, leftb, rightb, value, valueb, slope);
			len = strlen(str);
			ASSendData((U8*)&str[0], len);
			Cpu_Delay100US(100);	
	    }
	    HmiUnsetHandler(oldHandler);			
		DrvDisableMotorControl();
		(void)DRV_SERVO_Disable();	
	}
				
}
/* END Test */
